#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/passthrough.h>//直通滤波
#include <pcl/filters/statistical_outlier_removal.h>//统计滤波
#include <pcl/filters/random_sample.h>//点云降采样
#include <pcl/filters/voxel_grid.h>//随机采样

ros::Publisher cloud_pub;

void laserCallback(sensor_msgs::PointCloud2ConstPtr lasercloudmsg){
   pcl::PointCloud<pcl::PointXYZI>::Ptr lasercloud_in(new pcl::PointCloud<pcl::PointXYZI>);
   pcl::PointCloud<pcl::PointXYZI>::Ptr lasercloud_filtered(new pcl::PointCloud<pcl::PointXYZI>);
    
    pcl::fromROSMsg(*lasercloudmsg,*lasercloud_in);

    //创建滤直通波器对象
    pcl::PassThrough<pcl::PointXYZI> pass;
    pass.setInputCloud(lasercloud_in);
    pass.setFilterFieldName("z"); // 滤波字段名被设置为z轴方向
    pass.setFilterLimits(0.0, 1000.0); // 设置在过滤方向上的过滤范围
    pass.setNegative(false); // 设置保留范围内的点还是过滤掉范围内的点，标志为false时保留范围内的点
    pass.filter(*lasercloud_filtered);

     // 创建统计滤波器对象
    // pcl::StatisticalOutlierRemoval<pcl::PointXYZI>  sor_OutRemove;  
    // //输入点云 
    // sor_OutRemove.setInputCloud (lasercloud_in); 
    // //设置在进行统计时考虑查询点邻近点数
    // sor_OutRemove.setMeanK (30);  
    // //设置判断是否为离群点的阈值
    // sor_OutRemove.setStddevMulThresh (1.0); 
    // // 执行滤波返回滤波后的点云 
    // sor_OutRemove.filter (*lasercloud_filtered);

    // //创建随机采样滤波器对象	
    // pcl::RandomSample< pcl::PointXYZI > rs;
    // //设置输入点云
    // rs.setInputCloud(lasercloud_in);	
    // //设置下采样要保留点云的点数
    // rs.setSample(20000);
    // //设置随机函数种子点
    // //rs.setSeed(1);
    // //执行下采样滤波	
    // rs.filter(*lasercloud_filtered);



    sensor_msgs::PointCloud2 cloud_filtered_msg;
    pcl::toROSMsg(*lasercloud_filtered,cloud_filtered_msg);
    cloud_filtered_msg.header.stamp=ros::Time::now();
    cloud_filtered_msg.header.frame_id="rslidar";

    // 发布滤波后的点云消息
    cloud_pub.publish(cloud_filtered_msg);
}

int main(int argc, char *argv[]){
    // 初始化ROS节点
    ros::init( argc ,argv , "passthrough" );
    ros::NodeHandle n;

    // 创建一个ROS发布者，用于发布滤波后的点云消息
    cloud_pub = n.advertise<sensor_msgs::PointCloud2>("filtered_cloud", 1);

    // 创建一个ROS订阅者，用于接收原始点云消息
    ros::Subscriber laser_sub = n.subscribe<sensor_msgs::PointCloud2>("/rslidar_points" ,10,laserCallback);

    ros::spin();
    return 0;
}
